/*
 * Controller.cpp
 *
 *  Created on: Apr 21, 2010
 *      Author: Soheil Damangir
 */

#include "Controller.h"

namespace Group5 {

PID_controller::PID_controller(int _Kp, int _Ti, int _Td, int _dt):
    dt(_dt), Td(_Td), Ti(_Ti), Kp(_Kp)
{
  e0 = 0;
  e1 = 0;
  e2 = 0;
  last = 0;
  set_point = 0;
}
inline void PID_controller::setPoint(int _setpoint)
{
  set_point = _setpoint;
}
void PID_controller::feed(int n)
{
  e2 = e1;
  e1 = e0;
  e0 = set_point - n;
  last += Kp * (e0 + e0*dt/Ti + e0*Td/dt - e1 - e1*2*Td/dt + e2*Td/dt);
}
int PID_controller::get_val()
{
  return last;
}
int PID_controller::next(int n)
{
  feed(n);
  return last;
}
PID_controller::~PID_controller() {}

}
